#include <ctime>
#include <math.h>
#include <iostream>
#include <thread>
#include <unistd.h>
#include <queue>

#include "headers/all_control_define.hpp"
#include "headers/predict_model.hpp"
#include "headers/CV.hpp"

#ifndef _WIN32
#include "headers/serial.hpp"
#define SERIAL_MODE_ON 1
#endif

#define DEBUG 0
#define SEND_RAW_MESSAGE 0

//-------------------------------------------------------------
//Mat queue settings
#if CAMERA_ON
	#define MSEC_PER_TICKS 16.66666666666 //unit:毫秒
#else
	#define MSEC_PER_TICKS (50.0/3.0)
#endif
#define MAX_FRAME_IN_QUEUE 5
//-------------------------------------------------------------

using namespace std;
using namespace cv;

string get_program_path(string program_full_path) {
	int i, N;
	N = program_full_path.size();
	string out_str(program_full_path);

	for (i = N - 1; i >= 0; i--) {
		out_str.pop_back();
		if (program_full_path[i] == '/' || program_full_path[i] == '\\')break;
	}
	N = out_str.size();
	for (i = 0; i < N; i++) {
		if(out_str[i] == '\\') out_str[i] = '/';
	}

	return out_str;
}

#if !CAMERA_ON
string video_name;
Mutex capture_lock;
cv::VideoCapture capture;
#define wait_frame(frame) \
capture >> frame; \
if (frame.empty()) { \
	capture_lock.lock(); \
	capture.open(video_name); \
	capture >> frame; \
	capture_lock.unlock(); \
}
#else
#include "headers/camera.hpp"
#endif

typedef int64_t tick_t;

struct CV_data_type{
	RotatedRect target_rrect;
	RotatedRect leap_rrect;
	Point2f center;
	tick_t target_timestamp;
	bool is_getted;
};
//---------------------------------------------------------------------------------------------------------
//Camera
#if CAMERA_ON
Mutex camera_lock;
bool camera_init_succeed = false;
bool camera_handel_process_running = true;
bool CAMERA_ERRO = false;
thread *camera_handel_thread_p;

void camera_handel_callback(){
	camera_lock.lock();

	re_init_camera:

	while (!camera_init_succeed) {
		camera_init_succeed = init_Camera();
		if(!camera_init_succeed)cout << "Open camera faile!" << endl;
	}

	CAMERA_ERRO = false;

	camera_lock.unlock();

	while(camera_handel_process_running){
		camera_lock.lock();
		if (CAMERA_ERRO){
			camera_lock.unlock();
			close_Camera();
			camera_init_succeed = false;
			goto re_init_camera;
		}
		camera_lock.unlock();
		usleep(500000);
	}

	close_Camera();
	camera_init_succeed = false;
}


void OpenCamraHandelThread(){
	camera_handel_process_running = true;
	camera_handel_thread_p = new thread(camera_handel_callback);
	while(!camera_handel_thread_p->joinable());
}

void CloseCamraHandelThread(){
	camera_handel_process_running = false;
	camera_handel_thread_p->join();
	delete [] camera_handel_thread_p;
}
#endif

Mutex CV_data_lock, CV_image_lock;
bool terminate_CV = false, CV_have_new_data = false, display_updated = false;
#if DISPLAY	
AccMat display(5,5,CV_8UC3);
#endif
#ifndef _WIN32
Mutex Aim_data_lock;
short Aim[2] = { 0,0 };
#endif

int64 PROGRAM_SRART_TICKS;
#define GET_TICKS_FROM_START ((clock_t)(cv::getTickCount()) - (clock_t)PROGRAM_SRART_TICKS)

AccMat frame_pre_proc;

bool CV_IS_RUNNING = true;

//---------------------------------------------------------------------------------------------------------
//MatQueue
typedef queue<Mat> MatQueue;
typedef queue<AccMat> AccMatQueue;
typedef queue<tick_t> TicksQueue;

AccMatQueue *UMQ_p;
TicksQueue *MTQ_p;
AccMat cur_tick_frame;
tick_t now_mat_ticks = 0; 

Mutex MatQueue_lock;
bool mat_get_is_running = true;

void mat_get_callback(){
	while(mat_get_is_running){
		#if !CAMERA_ON
		wait_frame(cur_tick_frame);
		#else
		camera_lock.lock();
		CAMERA_ERRO = !(wait_frame(cur_tick_frame));

		if(CAMERA_ERRO){
			camera_lock.unlock();
			continue;
		}
		camera_lock.unlock();

		//cout << "CAMERA_ERRO: " << CAMERA_ERRO << endl;
		
		#endif
		
		MatQueue_lock.lock();
		//UMQ_p->push(cur_tick_frame);
		//MTQ_p->push(now_mat_ticks);
		if(UMQ_p->size() <= MAX_FRAME_IN_QUEUE){
			//UMQ_p->pop();
			//MTQ_p->pop();
			UMQ_p->push(cur_tick_frame);
                	MTQ_p->push(now_mat_ticks);
		}
		MatQueue_lock.unlock();
		now_mat_ticks++;
		usleep(2000);
	}
}

thread *MatGetThread_p;

void OpenMatQueueThread(){
	UMQ_p = new AccMatQueue();
	MTQ_p = new TicksQueue();
	mat_get_is_running = true;

	MatGetThread_p = new thread(mat_get_callback);
	while(!MatGetThread_p->joinable());
}

void CloseMatQueueThread(){
	mat_get_is_running = false;
	MatGetThread_p->join();
	delete [] MatGetThread_p;
}

bool get_mat_from_queue(AccMat &out_mat, tick_t &mat_tick){
	MatQueue_lock.lock();	

	#if CAMERA_ON
	camera_lock.lock();
	if(CAMERA_ERRO){
		MatQueue_lock.unlock();
		camera_lock.unlock();
		return false;
	}
	camera_lock.unlock();
	#endif
	
	if(UMQ_p->size() < 1){
		MatQueue_lock.unlock();
		return false;
	}
	if((UMQ_p->front()).empty()){
		MatQueue_lock.unlock();
		return false;
	}
	
	(UMQ_p->front()).copyTo(out_mat);
	UMQ_p->pop();
	mat_tick = MTQ_p->front();
	MTQ_p->pop();

	MatQueue_lock.unlock();
	return true;
}
//---------------------------------------------------------------------------------------------------------
//CVDataSet
struct CVDataComp{
	bool operator() (const CV_data_type &a, const CV_data_type &b){
		return a.target_timestamp < b.target_timestamp;
	}
};

typedef priority_queue<CV_data_type, vector<CV_data_type>, CVDataComp> CVDataSetType;

CVDataSetType CVDataSet;
Mutex CVDataSet_lock;

void insertCVData_to_set(CV_data_type &data){
	CVDataSet_lock.lock();
	CVDataSet.push(CV_data_type(data));
	if(CVDataSet.size() > MAX_FRAME_IN_QUEUE)CVDataSet.pop();
	CVDataSet_lock.unlock();
}

bool pop_CVData_from_set(CV_data_type &data){
	CVDataSet_lock.lock();
	if(CVDataSet.size() < 1){
		CVDataSet_lock.unlock();
		return false;
	}
	data = CVDataSet.top();
	CVDataSet.pop();
	CVDataSet_lock.unlock();
	return true;
}
//---------------------------------------------------------------------------------------------------------
//CV_main_process
Mutex image_get_lock, ROI_rect_lock;
Mat arm_template;

Rect frame_ROI; 
Point2f ROI_tl, ROI_br; 
double ROI_radius; 

void CV_main_process(void* _data) {
	CV_data_lock.lock();
	CV_data_type* data_p = (CV_data_type*)_data;

	data_p->is_getted = false;
	data_p->target_rrect = RotatedRect();
	data_p->center = Point2f();
	data_p->target_timestamp = (clock_t)0;

	clock_t start_t = 0, passed_t = 0;
	double passed_time = 0;
	CV_data_lock.unlock();
	//-----------------------------------------------------------------------------------------------------------------
	//LOAD_RESOURCES
	CV_data_lock.lock();
	AccMat arm_template_AccMat;
	arm_template.copyTo(arm_template_AccMat);
	if(arm_template.channels() == 3)cvtColor(arm_template, arm_template, CV_BGR2GRAY); 
	Mat template_feat; 
	get_item_feature(arm_template, template_feat);
	CV_data_lock.unlock();
	//-----------------------------------------------------------------------------------------------------------------
	//DEFINE_NEDDED_VAR
	AccMat ori_frame, roi_frame; 
	int Input_key, i = 0; 
	double match_result = -1, path_radius = 10; 
	RotatedRect getted_ammmor_rrect, getted_leap_rrect; 
	Point ammor_center; 
	Point2f rrect_points[4], center_point, ammor2center;
	//-----------------------------------------------------------------------------------------------------------------

	tick_t now_frame_tick = 0;
#if !CAMERA_ON
	tick_t temp_tick = 0;
#endif

	while (CV_IS_RUNNING) {
		CV_data_lock.unlock();

		image_get_lock.lock();
#if CAMERA_ON

		if(!get_mat_from_queue(ori_frame, now_frame_tick))continue;
#else	
		
		if(!get_mat_from_queue(ori_frame, now_frame_tick))continue;

#endif
		if(ori_frame.empty()){
			image_get_lock.unlock();
			continue;
		}
		image_get_lock.unlock();
#ifndef _WIN32

#if SERIAL_MODE_ON
		Aim_data_lock.lock();
		Aim[0] = ori_frame.cols / 2 - STATIC_COOR_YAW;
		Aim[1] = ori_frame.rows / 2 - STATIC_COOR_PICH;
		Aim_data_lock.unlock();
#endif
#endif
		ROI_rect_lock.lock();
		if (frame_ROI.size().area() < 25 || match_result == -1) {
			frame_ROI.height = ori_frame.rows;
			frame_ROI.width = ori_frame.cols;
			frame_ROI.x = 0; frame_ROI.y = 0;
		}
		else {
			ROI_radius = std::max(getted_ammmor_rrect.size.height, getted_ammmor_rrect.size.width) + path_radius;
			frame_ROI.height = ROI_radius * 2;
			ROI_tl.x = center_point.x - ROI_radius; ROI_tl.y = center_point.y - ROI_radius;
			ROI_br.x = center_point.x + ROI_radius; ROI_br.y = center_point.y + ROI_radius;
			if (ROI_tl.x < 0)ROI_tl.x = 0;
			if (ROI_tl.y < 0)ROI_tl.y = 0;
			if (ROI_br.x > ori_frame.cols - 1)ROI_br.x = ori_frame.cols - 1;
			if (ROI_br.y > ori_frame.rows - 1)ROI_br.y = ori_frame.rows - 1;
			frame_ROI = Rect(ROI_tl, ROI_br);
		}

		ROI_tl = frame_ROI.tl(); ROI_br = frame_ROI.br();
		
		ori_frame(frame_ROI).copyTo(roi_frame);
		ROI_rect_lock.unlock();
		
		CV_data_lock.lock();
		if(!Pre_proc(roi_frame, frame_pre_proc)){
			match_result = -1;
			CV_data_lock.unlock();
			continue;
		}
		CV_data_lock.unlock();

		//printf("\nCV_2\n");

#if DISPLAY && 0
		show_image("frame_pre_proc", frame_pre_proc, 800);
#endif
		//match_result = get_and_match(frame_pre_proc, arm_template, template_feat, getted_ammmor_rrect, getted_leap_rrect, center_point, MATCH_RELATIVE_ERR);
		match_result = get_and_match_AccMat(frame_pre_proc, arm_template_AccMat, template_feat, getted_ammmor_rrect, getted_leap_rrect, center_point, MATCH_RELATIVE_ERR);
		cout << "match_result: " << match_result << endl;

		ROI_rect_lock.lock();
		getted_ammmor_rrect.center = getted_ammmor_rrect.center + ROI_tl;
		center_point = center_point + ROI_tl;
		getted_leap_rrect.center = getted_leap_rrect.center + ROI_tl;
		ROI_rect_lock.unlock();

		getted_ammmor_rrect.points(rrect_points);

		ammor2center = getted_ammmor_rrect.center - center_point;
		path_radius = std::sqrt(ammor2center.x * ammor2center.x + ammor2center.y * ammor2center.y);

#if DISPLAY	
		CV_image_lock.lock();
		ori_frame.copyTo(display);
		/*
		line(display, rrect_points[0], rrect_points[1], Scalar(0, 255, 0), 2);
		line(display, rrect_points[1], rrect_points[2], Scalar(0, 255, 0), 2);
		line(display, rrect_points[2], rrect_points[3], Scalar(0, 255, 0), 2);
		line(display, rrect_points[3], rrect_points[0], Scalar(0, 255, 0), 2);
		*/
		ROI_rect_lock.lock();
		rectangle(display, frame_ROI, Scalar(0, 127, 255), 3);
		ROI_rect_lock.unlock();

		circle(display, center_point, 8, Scalar(0, 255, 255), -1);
		circle(display, getted_ammmor_rrect.center, 8, Scalar(0, 255, 127), -1);

		display_updated = true;
	
		/*show_image("CV_display", display, 800);
		Input_key = waitKey(1);*/

		CV_image_lock.unlock();

		//printf("\nCV_3\n");

#endif
		CV_data_lock.lock();
		CV_have_new_data = true;
		if (terminate_CV)break;
		data_p->is_getted = match_result != -1;
		if (!data_p->is_getted) {
			data_p->target_rrect = RotatedRect();
			data_p->leap_rrect = RotatedRect();
			data_p->center = Point2f(-1, -1);
			data_p->target_timestamp = now_frame_tick;
			insertCVData_to_set(*data_p);
			CV_data_lock.unlock();
			continue;
		}
		data_p->target_rrect = getted_ammmor_rrect;
		data_p->leap_rrect = getted_leap_rrect;
		data_p->center = center_point;
		data_p->target_timestamp = now_frame_tick;
		insertCVData_to_set(*data_p);
		CV_data_lock.unlock();

		//printf("\nCV_4\n");
		CV_data_lock.lock();
	}

	return;
}

double calc_passed_times(int64 *start_ticks_handle, int mode = 1){
	static int64 passed_ticks, now_tick;
	now_tick = cv::getTickCount();
	passed_ticks = now_tick - *start_ticks_handle;
	if(mode==0)*start_ticks_handle = now_tick;
	return (double)passed_ticks / cv::getTickFrequency();
}

int main(int argc, char** argv) {
	char* link = new char[512];
	ssize_t link_return_size;
	link_return_size = readlink("/proc/self/exe", link, 512);
	program_root_path = get_program_path(string(link));
	//cout << endl << "program_root_path: " << program_root_path << endl;
	PROGRAM_SRART_TICKS = (clock_t)getTickCount();
	arm_template.release();
	while(arm_template.empty())arm_template = imread(program_root_path + "/../resources/template/1/1.tif"); 

#if !CAMERA_ON
	video_name = program_root_path + "/../resources/red_low_expo.mp4";
	//video_name = program_root_path + "/../resources/2022_9_7_FAN.avi";
	//video_name = program_root_path + "/../resources/video_9_14.avi";
	capture.open(video_name);
#endif

	CV_data_type CV_data1, CV_data2, temp_CVData;
	int64 now_timestamp = 0, pre_timestamp = 0, pass_timestamp = 0;
	double now_time = 0.0;

	double path_radius = 0, predict_angle = 0;
	Point2f now_center, now_target, now_leap_center, predict_target, predict_target_correct;
	Point2f predict_linear_vel(0,0);
	RotatedRect now_target_rrect;
	Point2f center2target_vec;

	double calc_used_time = 0;
	double proc_times = 30;

	int i, N;
	bool target_getted = false, data_calculated = false, can_shoot = false;
	Point2f Ture_with_Correct_predict_point_error;

#if DEBUG && DISPLAY
	namedWindow("predict time");
	createTrackbar("time(ms)", "predict time", &time_ms, 5000);
#endif

#ifndef _WIN32
	short now_Position[2] = { 0, 0 }, sum_check = 0;
	float PosChange[2] = {0,0}, PrePos[2] = {-1,-1};
	float now_predict_Linear_vel[2] = {0, 0};
	unsigned char OutBuffer[17] = { 0 }, * Data_ptr;
#if SERIAL_MODE_ON
	bool serial_is_inited = false, position_data_can_get = false;
	//message ptr
	//OutBuffer?? |1 char(0x5b)|1 char|4 char(short[2])|1 char|8 char(float[2])|2 char (short)| \
	|head check|target getted|position|passed time|linear vel|tail check(sum check)|
	serial_is_inited = init_serial();
#endif
#endif

	CV_data1.target_timestamp = (int64)0;
	CV_data2.target_timestamp = (int64)0;
	pre_timestamp = (int64)0;
#if CAMERA_ON
	OpenCamraHandelThread();
#endif
	OpenMatQueueThread();

	thread CV_main_thread1(CV_main_process, (void*)&CV_data1);
	thread CV_main_thread2(CV_main_process, (void*)&CV_data2);
	
	while(!CV_main_thread1.joinable() || !CV_main_thread2.joinable());
	
	PredictModel PModel(PREDICT_MODEL_RANK);
	PredictModelFit PModelFit = PredictModelFit();

	bool terminate_main_process = false;

	int detected_times = 0;

	while(!terminate_main_process){
//------------------------------------------------------------------------------------------------------------------------
		//printf("\nPRED_1\n");

		CVDataSet_lock.lock();
		if(CVDataSet.size() < 1){
			CVDataSet_lock.unlock();
			//usleep(20000);
			//target_getted = false;
			//goto Send_Messages;
			continue;
		}

		pop_CVData_from_set(temp_CVData);

		target_getted = temp_CVData.is_getted;
		now_center = temp_CVData.center;
		now_leap_center = temp_CVData.leap_rrect.center;
		now_target_rrect = temp_CVData.target_rrect;
		now_timestamp = temp_CVData.target_timestamp;
		CV_have_new_data = false;

		CVDataSet_lock.unlock();

		now_time = (double)now_timestamp * (double)MSEC_PER_TICKS / 1000.0;

		//printf("\nPRED_2\n");
//------------------------------------------------------------------------------------------------------------------------
#if CONMMAND_LINE_SHOW_ON
		static int64 used_times_per_frame_ticks = cv::getTickCount();
		static double used_times_per_frame, temp_time;
		used_times_per_frame = calc_passed_times(&used_times_per_frame_ticks, 0) * 1000;
		cout << "Use times per frame: " << used_times_per_frame << endl;
#endif
		if(target_getted){
			if(detected_times < 3){
				detected_times++;
				continue;
			}
			temp_time = now_time*10.0;
			PModel.get_data(now_center, now_target_rrect.center, now_leap_center, temp_time);
			//cout << 1 << endl;
			PModelFit.get_data(now_center, now_target_rrect.center, now_leap_center, temp_time/10.0);
			//cout << 2 << endl;
		}else{
			detected_times = 0;
		}
		
#if CAMERA_ON
		calc_used_time = (double)(cv::getTickCount() - now_timestamp)/cv::getTickFrequency() * 10.0;
#else
		calc_used_time = 0.0;
#endif
		//printf("\nPRED_3\n");

		if(PModelFit.model_available){
			predict_angle = PModelFit.predict(PREDICT_TIME/1000.0);
			predict_target = PModelFit.get_predict_position();
			predict_angle = PModelFit.predict(PREDICT_TIME/1000.0 + CORRECT_TIME/1000.0);
			predict_target_correct = PModelFit.get_predict_position();
			//exit(0);
			/*
			CV_IS_RUNNING = false;
			CV_main_thread1.join();
			CV_main_thread2.join();
			return 1;
			*/
		}else{
			predict_angle = PModel.predict(PREDICT_TIME/100.0);
			predict_target = PModel.get_predict_position();
			predict_angle = PModel.predict(PREDICT_TIME/100.0 + CORRECT_TIME/100.0);
			predict_target_correct = PModel.get_predict_position();
		}
		predict_linear_vel = PModel.predict_vel(proc_times/100);
		proc_times = proc_times*0.9 + used_times_per_frame*0.1;

		static Point2f vec_tar2aim, vec_correct_tar2aim, Aim_pt;
		Aim_pt.x = (float)Aim[0];Aim_pt.y = (float)Aim[1];
		vec_tar2aim = predict_target - Aim_pt;
		vec_correct_tar2aim = predict_target_correct - Aim_pt;

		static double distance_of_target;
		distance_of_target = std::sqrt(vec_tar2aim.x*vec_tar2aim.x + vec_tar2aim.y*vec_tar2aim.y);
		can_shoot = distance_of_target < SHOOT_ZONE;

#if CONMMAND_LINE_SHOW_ON
		cout << "distance_of_target:" << distance_of_target << ' ';
#endif

		//printf("\nPRED_4\n");

//------------------------------------------------------------------------------------------------------------------------
	Send_Messages:

#ifndef _WIN32

		static int good_data_counter = 0;
		if(target_getted && good_data_counter < 4)good_data_counter++;
		else good_data_counter = 0;

		OutBuffer[0] = 0x5b;
		OutBuffer[1] = (unsigned char)((int)(target_getted && (good_data_counter > 3)) + (int)target_getted*(int)can_shoot);

		Aim_data_lock.lock();
		if (!target_getted) {
			if(PrePos[0] == -1){
				now_Position[0] = Aim[0];
				now_Position[1] = Aim[1];
				PrePos[0] = (float)Aim[0];
				PrePos[1] = (float)Aim[1];
			}else{
				now_Position[0] = (short)PrePos[0] + (short)PosChange[0];
				now_Position[1] = (short)PrePos[1] + (short)PosChange[1];
			}
			PosChange[0] = PosChange[0]*0.5f;
			PosChange[1] = PosChange[1]*0.5f;
			PrePos[0] = 0.4f*PrePos[0] + 0.6f*(float)Aim[0];
			PrePos[1] = 0.4f*PrePos[1] + 0.6f*(float)Aim[1];
		}
		else {
		#if SEND_RAW_MESSAGE
			now_Position[0] = short(now_target_rrect.center.x);
			now_Position[1] = short(now_target_rrect.center.y);
		#else 
			now_Position[0] = (short)predict_target_correct.x + (short)(vec_correct_tar2aim.x*CORRECT_MULTIPLIER_CORR_TAR + vec_tar2aim.x*CORRECT_MULTIPLIER_TRUE_TAR);
			now_Position[1] = (short)predict_target_correct.y + (short)(vec_correct_tar2aim.y*CORRECT_MULTIPLIER_CORR_TAR + vec_tar2aim.y*CORRECT_MULTIPLIER_TRUE_TAR);
		#endif
			if(PrePos[0] != -1){
				PosChange[0] = 0.5f*PosChange[0] + 0.5f*((float)now_Position[0] - PrePos[0]);
				PosChange[1] = 0.5f*PosChange[1] + 0.5f*((float)now_Position[1] - PrePos[1]);
			}
			PrePos[0] = (float)now_Position[0];
			PrePos[1] = (float)now_Position[1];
		}

		predict_linear_vel = predict_linear_vel*10.0f;
		now_predict_Linear_vel[0] = predict_linear_vel.x;
		now_predict_Linear_vel[1] = predict_linear_vel.y;

		now_Position[0] -= Aim[0]; now_Position[1] -= Aim[1];
		Aim_data_lock.unlock();

		/*for (i = 0; i < 4; i++) {	//??????д?? OutBuffer uchar????????
			Data_ptr = (uchar*)now_Position;
			OutBuffer[5 - i] = Data_ptr[i];
		}*/

		OutBuffer[2] = (uchar)(now_Position[0] >> 8);
		OutBuffer[3] = (uchar)(now_Position[0]);
		OutBuffer[4] = (uchar)(now_Position[1] >> 8);
		OutBuffer[5] = (uchar)(now_Position[1]);

		OutBuffer[6] = uchar(((double)GET_TICKS_FROM_START/(double)getTickFrequency() - (double)now_time) * 1000);

		Data_ptr = (uchar*)((void*)now_predict_Linear_vel);
		for(i=0;i<8;i++){
			OutBuffer[7+i] = Data_ptr[i];
		}

		sum_check = short(now_Position[0]) + short(now_Position[1]) + short(OutBuffer[1]) + short(OutBuffer[6]) + short(now_predict_Linear_vel[0]) + short(now_predict_Linear_vel[1]);

		OutBuffer[15] = (uchar)(sum_check >> 8);
		OutBuffer[16] = (uchar)(sum_check);

		#if CONMMAND_LINE_SHOW_ON
		cout << "Vision center = (" << Aim[0] << ',' << Aim[1] << ") ";
		cout << "Target output = (" << now_Position[0] << ',' << now_Position[1] << ')' << endl;
		cout << "now_predict_Linear_vel = (" << now_predict_Linear_vel[0] << "," << now_predict_Linear_vel[1] << ")\n";
		cout << "sum_check: " << int(sum_check) << endl;
		#endif

		//printf("\nPRED_5\n");

#if SERIAL_MODE_ON
		static bool send_error = false;
		send_error = send_message(OutBuffer, 17) < 0;

		if (!serial_is_inited || send_error) {
			printf("Unable to send message to serial.\n");
			close_serial();
			serial_is_inited = init_serial();
		}
#endif
#endif
//------------------------------------------------------------------------------------------------------------------------
#if DISPLAY
		static Point2f send_point;
		send_point.x = float(now_Position[0]+Aim[0]);
		send_point.y = float(now_Position[1]+Aim[1]);

		CV_image_lock.lock();
		if(display_updated){
			circle(display, predict_target, 8, Scalar(255, 0, 255), -1);
			circle(display, send_point, 15, Scalar(60, 200, 100), 5);
			if(!can_shoot)circle(display, predict_target_correct, 10, Scalar(63, 63, 255), 3);
			else circle(display, predict_target_correct, 10, Scalar(127, 255, 127), 3);
			circle(display, Point((int)Aim[0], (int)Aim[1]), 10, Scalar(0, 0, 255), -1);
			circle(display, now_leap_center, 10, Scalar(31,150,255),-1);
			line(display, now_target_rrect.center, now_target_rrect.center + predict_linear_vel/10.0, Scalar(0,0,255),5);
			show_image("display", display, 800);
			waitKey(1);
			display_updated = false;
		}
		CV_image_lock.unlock();

		//printf("\nPRED_6\n");
#endif
//------------------------------------------------------------------------------------------------------------------------
pre_timestamp = now_timestamp;

	}

	CV_data_lock.lock();
	terminate_CV = true;
	CV_data_lock.unlock();
	CV_main_thread1.join();
	CV_main_thread2.join();
#ifndef _WIN32
#if SERIAL_MODE_ON
	close_serial();
#endif
#endif
#if CAMERA_ON
	close_Camera();
#endif

	return 0;
}
